#include "../Device/maintask.h"

#include <stdio.h>
#include <string.h>

#include "../Device/cmd.h"
#include "main.h"

extern float power_precentage;
Status_t status = STATUS_STOP;
char maintask_tmp[26];
extern float Pitch, Roll;
extern float power_precentage;
float Roll_I;
extern int16_t compass_data_x, compass_data_y, compass_data_z;

void Update_LED() {
  static uint32_t counter = 0;
  if (counter > MAINTASK_FREQ / 1)
    counter = 0;
  else
    counter++;

  if (HAL_GPIO_ReadPin(MOTOR_SWITCH_GPIO_Port, MOTOR_SWITCH_Pin))
    status = STATUS_WORK;
  else
    status = STATUS_STOP;

  switch (status) {
    case STATUS_STOP:
      if (counter > MAINTASK_FREQ / 10.0f)
        HAL_GPIO_WritePin(Board_LED_GPIO_Port, Board_LED_Pin, GPIO_PIN_SET);
      else
        HAL_GPIO_WritePin(Board_LED_GPIO_Port, Board_LED_Pin, GPIO_PIN_RESET);
      break;
    case STATUS_WORK:
      if (counter > MAINTASK_FREQ / 1.5f)
        HAL_GPIO_WritePin(Board_LED_GPIO_Port, Board_LED_Pin, GPIO_PIN_SET);
      else
        HAL_GPIO_WritePin(Board_LED_GPIO_Port, Board_LED_Pin, GPIO_PIN_RESET);
      break;
  }

  if (power_precentage > 0.8f) {
    HAL_GPIO_WritePin(POWER_LED_5_GPIO_Port, POWER_LED_5_Pin, GPIO_PIN_SET);
  } else
    HAL_GPIO_WritePin(POWER_LED_5_GPIO_Port, POWER_LED_5_Pin, GPIO_PIN_RESET);

  if (power_precentage > 0.6f) {
    HAL_GPIO_WritePin(POWER_LED_3_GPIO_Port, POWER_LED_3_Pin, GPIO_PIN_SET);
  } else
    HAL_GPIO_WritePin(POWER_LED_3_GPIO_Port, POWER_LED_3_Pin, GPIO_PIN_RESET);

  if (power_precentage > 0.4f) {
    HAL_GPIO_WritePin(POWER_LED_2_GPIO_Port, POWER_LED_2_Pin, GPIO_PIN_SET);
  } else
    HAL_GPIO_WritePin(POWER_LED_2_GPIO_Port, POWER_LED_2_Pin, GPIO_PIN_RESET);

  if (power_precentage > 0.2f) {
    HAL_GPIO_WritePin(POWER_LED_1_GPIO_Port, POWER_LED_1_Pin, GPIO_PIN_SET);
  } else
    HAL_GPIO_WritePin(POWER_LED_1_GPIO_Port, POWER_LED_1_Pin, GPIO_PIN_RESET);

  if (power_precentage > 0.0f) {
    HAL_GPIO_WritePin(POWER_LED_4_GPIO_Port, POWER_LED_4_Pin, GPIO_PIN_SET);
  } else
    HAL_GPIO_WritePin(POWER_LED_4_GPIO_Port, POWER_LED_4_Pin, GPIO_PIN_RESET);
}
void Report_Status() {
  sprintf(maintask_tmp, "%d,%d,%d,%.2f\r\n", compass_data_x, compass_data_y,
          compass_data_z, power_precentage);
  Uart_SendDMA((uint8_t *)maintask_tmp, strlen(maintask_tmp));
}

void Switch_Control_Stby() {
  if (HAL_GPIO_ReadPin(MOTOR_SWITCH_GPIO_Port, MOTOR_SWITCH_Pin))
    HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_SET);
  else
    HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_RESET);
}
